#include<iostream>
#include<thread>
#include <mutex>
#include <condition_variable>
//#include <conio.h>
#include "keyboard.h"
#include <chrono>
using namespace std;
#include "car.h"
mutex syn_mutex;
condition_variable cv;
string c;
bool work1Finshed = false;
bool work2Finshed = false;
bool work3Finshed = false;
bool work4Finshed = true;
void readkeyboard()
{
    if(kbhit())
        {
            c = fgetc(stdin);
            cout<<endl;
        }
}
void f1()
{   
    while(true)
    {
        unique_lock<mutex> lock(syn_mutex);
        cv.wait(lock,[]{return work4Finshed;});
       
        Dipan d;
        Lidar l;
        l.attach(&d);
        readkeyboard();
        l.setState(c);
        cout<<"前左  ";
        l.Notify();
        work1Finshed=true;
        work2Finshed=false;
        work3Finshed=false;
        work4Finshed=false;
        cv.notify_all();
    this_thread::sleep_for(chrono::seconds(1));
    }
 
}
void f2()
{   
    while(true)
    {
        unique_lock<mutex> lock(syn_mutex);
        cv.wait(lock,[]{return work1Finshed;});
        
        Dipan d;
        Lidar l;
        l.attach(&d);
        readkeyboard();
        l.setState(c);
        cout<<"前右  ";
        l.Notify();
        work1Finshed=false;
        work2Finshed=true;
        work3Finshed=false;
        work4Finshed=false;
        cv.notify_all();
     this_thread::sleep_for(chrono::seconds(1));
    }
}
void f3()
{   
    while(true)
    {
        unique_lock<mutex> lock(syn_mutex);
        cv.wait(lock,[]{return work2Finshed;});
        
        Dipan d;
        Lidar l;
        l.attach(&d);
        readkeyboard();
        l.setState(c);
        cout<<"后左  ";
        l.Notify();
        work1Finshed=false;
        work2Finshed=false;
        work3Finshed=true;
        work4Finshed=false;
         
        cv.notify_all();
    this_thread::sleep_for(chrono::seconds(1));
    }

}
void f4()
{   
    while(true)
    {
        unique_lock<mutex> lock(syn_mutex);
        cv.wait(lock,[]{return work3Finshed;});
        
        Dipan d;
        Lidar l;
        l.attach(&d);
        readkeyboard();
        l.setState(c);
        cout<<"后右  ";
        l.Notify();
        work1Finshed=false;
        work2Finshed=false;
        work3Finshed=false;
        work4Finshed=true;
         
        cv.notify_all();
    this_thread::sleep_for(chrono::seconds(1));
    }
    
}

int main()
{
    
    cout<<"请输入障碍方位（前方:2，左前方:3，右前方:4）或出发(前进:1)"<<endl;
    cin>>c;
    while(true)
    {
       
        thread t1(f1),t2(f2),t3(f3),t4(f4);
        t1.join();
        t2.join();
        t3.join();
        t4.join();
    }
  
    return 0;
}